/*
 * trajectory.hpp
 *
 *  Created on: 2021年5月3日
 *      Author: zhiping fu
 */

#ifndef MODULES_TRAJECTORY_TRAJECTORY_HPP_
#define MODULES_TRAJECTORY_TRAJECTORY_HPP_


#include "system/system.hpp"
#include "userlib/userlib.hpp"

#ifdef __cplusplus
extern "C" {
#endif

#define MAX_WAYPOINTS_NUM 20		//最大航点数量
#define MAX_SEG_NUM		19
#define n_order			7		//轨迹阶数

typedef enum
{
	n_state_x   = 1,
	n_state_y   = 2,
	n_state_z   = 3,
	n_state_psi = 4,
}eN_State;

#ifdef __cplusplus
}
#endif

#ifdef __cplusplus
#include <Eigen>

#include <math.h>
using namespace Eigen;
using namespace std;


class TRAJECTORY
{
public:
	TRAJECTORY(){}
	TRAJECTORY(char * n) : name(n){}
	~TRAJECTORY(){}

	void trajectory_Init(void);
	void trajectory_Run(void);

	void mininumSnapCloseSolver(eN_State n_state);

	void getQ(void);
	void getM(void);
	void getCt(void);



private:
	char* name;
	float volcity_average;			//无人机平均速度，人为设定
	float dis_sum;					//轨迹段的总距离
	float T_sum;					//无人机轨迹跟踪总时间
	uint8_t numOfWaypoints;			//航点数量
	uint8_t n_seg;					//轨迹段数

	float dis_nth[MAX_SEG_NUM];					//每段轨迹的长度
	float T_nth_seg[MAX_SEG_NUM];				//每段轨迹的时间

	MatrixXf    Q;
	VectorXf    P;
	MatrixXf    Ct;
	MatrixXf    R;
	MatrixXf    M;
	VectorXf    dF;
	VectorXf    dP;
	VectorXf    dF_dP;

	Matrix<float, 8, (n_order+1)> M_nth;
	Matrix<float, (n_order+1), (n_order+1)> Q_nth;



	Inloop_control_msg inloop_control;
	height_msg height;

	uint32_t  startTimer;			//计时器
	uint32_t  stopTimer;			//计时器
	uint32_t  executionTime_us;			//计时器
	uint32_t  startTimerLast;
	uint32_t  cycleTime_us;




};

#endif

#endif /* MODULES_TRAJECTORY_TRAJECTORY_HPP_ */
